Adaptive cubature Kalman filter based on the variance-covariance components estimation

Although the Kalman filter (KF) is widely used in practice, its estimated results are optimal only when the system model is linear and the noise characteristics of the system are already exactly known. However, it is extremely difficult to satisfy such requirement since the uncertainty caused by the inertial instrument and the external environment, for instance, in the aided inertial navigation. In practice almost all of the systems are nonlinear. So the nonlinear filter and the adaptive filter should be considered together. To improve the filter accuracy, a novel adaptive filter based on the nonlinear Cubature Kalman filter (CKF) and the Variance-Covariance Components Estimation (VCE) was proposed in this paper. Here, the CKF was used to solve the nonlinear issue while the VCE method was used for the noise covariance matrix of the nonlinear system real-time estimation. The simulation and experiment results showed that better estimated states can be obtained with this proposed adaptive filter based on the CKF. Keyword: Adaptive filter, Cubature Kalman filter (CKF), Variance-Covariance Components Estimation (VCE), Nonlinear system, Noise covariance matrix Introduction In modern navigation and data fusion, the Kalman filter (KF) has become one of the most widely used estimation methods due to its advantages of being more simple and useful (Chen 2012; Han & Wang 2012; Feng et al. 2013; Santos 2015). However, the KF method still has some limitations (Tang et al. 2014; Wang et al. 2010; Wang 2009). For example, it has been developed on the base of linear systems while almost all of the systems are nonlinear actually. So if the KF is applied in practice, the nonlinearity may lead to large errors or even to the filter divergence. Thus, the nonlinear filters have been a hot area of the state estimation. In practical engineering, two nonlinear methods named the extended Kalman filter (EKF) and the unscented Kalman filter (UKF) are most widely used (Gao et al. 2014; Dini et al. 2011; Vaccarella et al. 2013; Masazade et al. 2012). In the EKF, the nonlinear system * Correspondence: [email protected] College of Information and Communication Engineering, Harbin Engineering University, Harbin 150001, China Full list of author information is available at the end of the article © The Author(s). 2017 Open Access This article International License (http://creativecommons.o reproduction in any medium, provided you giv the Creative Commons license, and indicate if can be linearized utilizing the Taylor series expansion for variance propagation while the prediction of the state vector and measurement vector are conducted using the nonlinear system (Gao et al. 2014; Arasaratnam & Haykin 2009). Although this method is used in many nonlinear systems for its simplicity, the precision is limited in the systems with strong nonlinearity and the fussy Jacobian matrix should be calculated which will inevitably increase the computational load. With the Unscented Transformation (UT), the UKF method can approximate the mean and the variance of the Gaussian state distribution using the nonlinear function to avoid the local linearization and the calculation of the Jacobian matrix effectively (Gao et al. 2014; Dini et al. 2011). However, the covariance matrix sometimes is easy non-positive in high-dimensional systems which will lead to filtering divergence. To solve the above mentioned problems, Arasaratnam et al. proposed the Cubature Kalman filter (CKF) method based on the Cubature Transform to conduct the prediction using the Cubature points which have the same weight (Arasaratnam & Haykin 2009; Arasaratnam is distributed under the terms of the Creative Commons Attribution 4.0 rg/licenses/by/4.0/), which permits unrestricted use, distribution, and e appropriate credit to the original author(s) and the source, provide a link to changes were made. Zhang et al. The Journal of Global Positioning Systems (2017) 15:1 Page 2 of 9 & Haykin 2010; Zhang et al. 2015). Similar with UKF, CKF avoids linearization to the nonlinear system by utilizing point sets to predict state vectors and covariance matrix. However, CKF has strictly theoretical derivation based on Bayesian and Spherical-Radial Cubature principles while UKF does not. In addition, the point set in CKF is acquired by integration and all weights are positive values and equal while the weights in UKF are negative values easily in high dimensional systems, which will reduce the filtering accuracy and stability. So the CKF is more widely used in practical to solve the nonlinear problem. In above filters, the estimations are the optimal only when the mathematic model is exactly known and the system process and measurement noises are the White Gaussian Noise. However, this cannot easily be satisfied in practical aided inertial navigation applications, for example, because of the drift errors of the inertial components, the dynamic errors caused by the carrier’s maneuver, and the existence of the uncertainty in the external environment (Wang et al. 2010; Jin et al. 2014; Von Chong & Caballero 2014). The system model and its associated statistical characteristics of the noises are obtained based on the priori knowledge, which definitely have some errors compared with the true values. So the adaptive filter should be considered to improve the estimated accuracy. The adaptive filter can estimate and correct the model parameters and the noise characteristics at the same time to continually reduce the errors of the state estimation. Then the estimated accuracy can be improved availably (Hu et al. 2003; Chang & Liu 2015; Mundla et al. 2012). At present, there are various adaptive filters. One of the outstanding adaptive filters named the Sage-Husa adaptive filter can estimate variance matrix of the process noise or the one of the observation noise in real-time when the variance matrix of the observation noise or the one of the process noise is known (Mundla et al. 2012). In other words, if we want to estimate the process (or observation) noise variance matrix with the Sage-Husa adaptive filter, variance matrix of the observation noise (or the process noise) should be already known. Thus, these two variance matrices cannot be estimated simultaneously. It has limitations when the process and observation noises are both unknown. By adjusting or restraining the impacts of the current observations on the parameter estimation using the fading factor, an adaptive filter makes full use of the current observations to improve the filtering accuracy (Chang & Liu 2015). Nevertheless, it is difficult to obtain the optimal fading factor and the computational complexity will accordingly be increased as well. Furthermore, the weights of the a-priori reliable information would be decreased to a certain extent if the current observation is abnormal, and then the filtering accuracy would also be degraded. To estimate the parameters of the system correctly, the weights of different observations should be known. Thus, the Variance-Covariance Components Estimation (VCE) was proposed (Wang et al. 2010; Moghtased-Azar et al. 2014). At present, various VCE methods have widely be used in statistics and geodesy. And one of the famous VCE methods is a posteriori VCE method after Helmert, in which the weights of different observations can be calculated by the adaptive iteration. Since the matrix will be negative sometimes during the calculation, some improved methods were proposed. Wang et al. proposed an adaptive Kalman filter (AKF) based on the VCE method, and verified its effectiveness using the actual experiments (Wang et al. 2010; Wang 2009; Wang et al. 2009). Although the adaptive filter based on the VCE has many advantages in the filter estimations, its application in nonlinear systems has rarely been studied. Thus, by combining the CKF and the VCE method, an improved adaptive filter was proposed in this paper. Since the adaptive filter and the nonlinear filter were engaged at the same time, this novel method can not only estimate the statistical property of the system’s noise, but also be applied in various nonlinear systems. Hence, the accuracy and the applicability of the new filter can be further improved. The rest of this paper was organized as follows. The description of the CKF and the VCE method was presented in Basic Knowledge. An improved adaptive filter based on the CKF proposed the novel adaptive filter based on the CKF. Numerical examples and experiment along with specific analysis were given in Simulations and Experiments. Conclusions concluded this manuscript. Basic knowledge Adaptive filter based on the VCE method The adaptive filter based on the VCE method can estimate the variance components of the process noise and the measurement noise vectors in real time using the residual vectors to decompose the system innovation vector (Wang et al. 2010; Wang 2009; Moghtased-Azar et al. 2014). On the basis of the estimated variance components, the weighting matrices of the process noise and the measurement noise vectors can be adjusted and then their effects on the state vector can be adjusted accordingly. Now under the consideration of the standard system model, the state and measurement equations are: x k ð Þ 1⁄4 Φ k; k−1 ð Þx k−1 ð Þ þ Γ k ð Þw k ð Þ z k ð Þ 1⁄4 H k ð Þx k ð Þ þ Δ k ð Þ ð1Þ where x(k) and z(k) are the state vector and the measurement vector, respectively; Φ(k, k − 1), Γ(k) and H(k) are Zhang et al. The Journal of Global Positioning Systems (2017) 15:1 Page 3 of 9 the state-transition matrix, the coefficient matrix of the process noise vector and the design matrix, respectively; w(k) and Δ(k) denote the process noise vector and the measurement noise vector, respectively. Further, w(k) and Δ(k) are the zero mean Gaussian noises: w k ð ÞeN 0; Q k ð Þ ð Þ Δ k ð ÞeN 0; R k ð Þ ð Þ ð2Þ where Q(k) and R(k) are positive definite matrices. Thus, the two-step update process of the Kalman filter is as follows: The time update: x̂ k=k−1 ð Þ 1⁄4 Φ k; k−1 ð Þx̂ k−1 ð Þ PXX k=k−1 ð Þ 1⁄4 Γ k ð ÞQ k ð ÞΓT k ð Þ þΦ k; k−1 ð ÞPXX k−1 ð ÞΦT k; k−1 ð Þ ð3Þ The measurement update: x̂ k ð Þ 1⁄4 x̂ k=k−1 ð Þ þ G k ð Þd k ð Þ PXX k ð Þ 1⁄4 E−G k ð ÞH k ð Þ 1⁄2 PXX k=k−1 ð Þ ð4Þ where G(k) is the gain matrix and d(k) is the system innovation vector: G k ð Þ 1⁄4 PXX k k−1 j ð ÞHT k ð ÞPdd k ð Þ d k ð Þ 1⁄4 z k ð Þ−H k ð Þx̂ k k−1 j ð Þ Pdd k ð Þ 1⁄4 H k ð ÞPXX k k−1 j ð ÞHT k ð Þ þ R k ð Þ ð5Þ When Q(k) and R(k) are already known, the estimated state vector is optimal. However, in practice they cannot easily be obtained. So it would be great if they could be estimated in real time in the adaptive filter based on the VCE method. There exist three groups of stochastic information that is associated with the estimation of the state vector: the observation noise vector Δ(k), the system process noise w(k) and the noise on the predicted state vector x̂ k=k−1 ð Þ brought by x̂ k‐1 ð Þ though the propagation of {Δ(1),⋯,Δ(k − 1)} and {w(1),⋯,w(k ‐ 1)}. Thus, according to noise sources, we can define three independent (pseudo-)observation groups as follows (Wang 2009): lx k ð Þ 1⁄4 Φ k; k−1 ð Þx̂ k−1 ð Þ lw k ð Þ 1⁄4 w0 k ð Þ lz k ð Þ 1⁄4 z k ð Þ ð6Þ where lx(k) is the pseudo-observation related to the predicted state vector, lw(k) is the pseudo-observation related to the system process noise, and lz(k) is the pseudo-observation related to the observation noise. Using their residual equations, the system in Eq. (1) can be rewritten: vlx k ð Þ 1⁄4 x̂ k ð Þ þ Γ k ð Þŵ k‐1 ð Þ−lx k ð Þ vlw k ð Þ 1⁄4 ŵ k‐1 ð Þ−lw k ð Þ vlz k ð Þ 1⁄4 Η k ð Þx̂ k ð Þ−lz k ð Þ ð7Þ with their measurement variance matrices as follows: Plxlx k ð Þ 1⁄4 Φ k; k−1 ð ÞPxx k−1 ð ÞΦT k; k−1 ð Þ Plwlw k ð Þ 1⁄4 Q k ð Þ Plzlz k ð Þ 1⁄4 R k ð Þ ð8Þ So we can estimate the covariance matrix of the system as long as we calculate the covariance matrix of the residual vectors. According to the steps of the Kalman filter, the estimations of the residual vectors can be calculated by the equation (9): vlxlx k ð Þ 1⁄4 Plxlx k ð ÞP−1 XX k k−1 j ð ÞG k ð Þd k ð Þ vlwlw k ð Þ 1⁄4 Q k−1 ð ÞΓT k−1 ð ÞP−1 XX k k−1 j ð Þ⋅ G k ð Þd k ð Þ vlzlz k ð Þ 1⁄4 H k ð ÞG k ð Þ−E f gd k ð Þ 8<>>: ð9Þ And then the corresponding variance matrices are as below: Pvlxlx k ð Þ 1⁄4 Φ k−1 ð ÞPXX k−1 ð ÞΦT k−1 ð ÞHT k ð Þ⋅ P dd k ð ÞH k ð ÞΦ k−1 ð ÞPXX k−1 ð ÞΦT k−1 ð Þ Pvlwlw k ð Þ 1⁄4 Q k−1 ð ÞΓT k−1 ð ÞHT k ð ÞP−1 dd k ð Þ⋅ H k ð ÞΓ k−1 ð ÞQ k−1 ð Þ Pvlz lz k ð Þ 1⁄4 E−H k ð ÞG k ð Þ f gR k ð Þ 8>><>>>>>: ð10Þ Here the innovation vector is epochwise projected into three residual vectors associated with the three groups of the measurements. Hence, we can estimate the variance factors. Assume that all components in lz(k) and lw(k) are uncorrelated, so both of the R(k) and Q(k) become diagonal. In this case, the redundancy index of each measurement noise factor is given by (Wang 2009) rzi k ð Þ 1⁄4 1− H k ð ÞG k ð Þ f gii ð11Þ Similarly, the redundancy index of each process noise factor is equal to rwi k ð Þ 1⁄4 Q k ð ÞΓT k ð ÞHT k ð ÞP−1 dd k ð Þ⋅ H k ð ÞΓ k−1 ð Þ ii ð12Þ Furthermore, the individual group redundancy contributions, or the total group redundant indexes, are equal to (Wang et al. 2010; Wang 2009; Wang et al. 2009): Zhang et al. The Journal of Global Positioning Systems (2017) 15:1 Page 4 of 9 rx k ð Þ 1⁄4 trace Φ k−1 ð ÞPXX k ð ÞΦT k−1 ð Þ⋅ HT k ð ÞP−1 dd k ð ÞH k ð Þ rw k ð Þ 1⁄4 trace Q k ð ÞΓT k ð ÞHT k ð ÞP−1 dd k ð Þ⋅ H k ð ÞΓ k−1 ð Þ rz k ð Þ 1⁄4 trace E−H k ð ÞG k ð Þ 1⁄2 8<>>: ð13Þ On the basis of the Herlmet VCE method, the individual group variance factors of unit weight are estimated by the residual vector and the corresponding redundant index as follows: σ̂ 0g k ð Þ 1⁄4 vg Plglg k ð Þvg k ð Þ rg k ð Þ g 1⁄4 x;w; z ð Þ ð14Þ Thus, at time k, the individual variance factors of lz(k) can be calculated by: σ̂ 2zi k ð Þ 1⁄4 vzi k ð Þ rzi k ð Þ ð15Þ And the covariance matrix of the measurement noise is as follows: R k ð Þ 1⁄4 σ̂ 2z1 k ð Þ ⋱ σ̂ 2zp k ð Þ 24 35 ð16Þ Similarly, the variance factors of lw(k) and the variance matrix Q(k) can be calculated by the equations (17) and (18): σ̂ 2wj k ð Þ 1⁄4 vwj k ð Þ rwj k ð Þ ð17Þ Q k ð Þ 1⁄4 σ̂ 2w1 k ð Þ ⋱ σ̂ 2wm k ð Þ 24 35 ð18Þ CKF algorithm Since the CKF is not only simple and easy to be implemented, but also high precise and convergent well, it is widely used in nonlinear estimations. Let us consider a nonlinear state-space model: xk 1⁄4 f xk−1 ð Þ þ wk−1 zk 1⁄4 h xk ð Þ þ Δk ( ð19Þ where xk and zk are the state vector and the measurement vector, respectively, f(⋅) and h(⋅) are the nonlinear state and measurement vector functions; w ∼N(0, Q) and Δ ∼N(0, R) represent the process noise and the measurement noise vectors, respectively. For this nonlinear system, 2n Cubature points which are equal of weight were selected to calculate the Gaussian distribution, and then the CKF can be implemented through the time and measurement updates. And the cubature points are set as: ξ i 1⁄4 ffiffiffiffiffi 2n 2 r 1 1⁄2 i ωi 1⁄4 1 2n ><>: ; i 1⁄4 1; 2;⋯; 2n ð20Þ where n is the dimension of the state vector. And the estimation process is as below (Arasaratnam & Haykin 2009; Arasaratnam & Haykin 2010): The time update: Pk−1 k−1 j 1⁄4 Sk−1 k−1 j S k−1 k−1 j ð21Þ Xi;k−1 k−1 j 1⁄4 Sk−1 k−1 j ξ i þ x̂k−1 k−1 j ð22Þ X i;k k−1 j 1⁄4 f Xi;k−1 k−1 j ð23Þ x̂k k−1 j 1⁄4 1 2N X2N i1⁄41 X i;k k−1 j ð24Þ Pk k−1 j 1⁄4 1 2N X2N i1⁄41 X i;k k−1 j X T i;k k−1 j −x̂k k−1 j x̂ T k k−1 j þ Qk−1 ð25Þ The measurement update: Pk k−1 j 1⁄4 Sk k−1 j S k k−1 j ð26Þ Xi;k k−1 j 1⁄4 Sk k−1 j ξ i þ x̂k k−1 j ð27Þ Yi;k k−1 j 1⁄4 h Xi;k k−1 j ð28Þ ẑk k−1 j 1⁄4 1 2N X2N i1⁄41 Yi;k k−1 j ð29Þ P k k−1 j 1⁄4 1 2N X2N i1⁄41 Yi;k k−1 j Y i;k k−1 j −ẑk k−1 j ẑ T k k−1 j þ Rk ð30Þ P k k−1 j 1⁄4 1 2N X2N i1⁄41 Xi;k k−1 j Y i;k k−1 j −x̂k k−1 j ẑ T k k−1 j ð31Þ And the gain matrix is: Kk 1⁄4 P k k−1 j P k k−1 j −1 ð32Þ The estimation of the state vector can be obtained: x̂k k j 1⁄4 x̂k k−1 j þ Kk zk−ẑk k−1 j ð33Þ where dk = zk − ẑk|k − 1 is the system innovation vector of the nonlinear system. The variance matrix of the estimated state vector is: Pk k j 1⁄4 Pk k−1 j −KkP k k−1 j K k ð34Þ Zhang et al. The Journal of Global Positioning Systems (2017) 15:1 Page 5 of 9 Methods An improved adaptive filter based on the CKF To solve the nonlinear problem and improve the stochastic model simultaneously, we here proposed a new improved adaptive filter by combining the CKF and the VCE adaptive method. With the nonlinear system described in Eq. (19), three pseudo measurement groups are defined as follows: lx k ð Þ 1⁄4 f x̂k‐1 k‐1 j lw k ð Þ 1⁄4 wk−1 lz k ð Þ 1⁄4 zk ><>: ð35Þ The residual equation of the nonlinear system is: vlx k ð Þ 1⁄4 x̂k k j þ ŵk−1−lx k ð Þ vlw k ð Þ 1⁄4 ŵk−1−lw k ð Þ vlz k ð Þ 1⁄4 h x̂k k j −lz k ð Þ ><>: ð36Þ According to the steps of the CKF, the equations (9) and (10) can be rewritten as: vlxlx k ð Þ 1⁄4 Plxlx k ð Þ Pk k−1 j −1 Kkdk vlwlw k ð Þ 1⁄4 Qk−1 Pk k−1 j −1 Kkdk vlzlz k ð Þ 1⁄4 P k k−1 j T Pk k−1 j −1 Kk−E dk 8<>>: ð37Þ The corresponding variance matrices are: Fig. 1 MSEs of 20 times of Monte Carlo simulations Pvlxlx k ð Þ 1⁄4 Plxlx k ð Þ Pk k−1 j −1 P k k−1 j P zz k k−1 j −1 ⋅ P k k−1 j T Pk k−1 j −1 Plxlx k ð Þ Pvlwlw k ð Þ 1⁄4 Qk−1 Pk k−1 j −1 P k k−1 j P zz k k−1 j −1 ⋅ P k k−1 j T Pk k−1 j −1 Qk−1 Pvlzlz k ð Þ 1⁄4 E− P k k−1 j T Pk k−1 j −1 Kk R k ð Þ 8>>>><>>>>>>>: ð38Þ The individual group redundant indices are equal to: rx k ð Þ 1⁄4 trace Plxlx k ð Þ Pk k−1 j −1 Pxz k k−1 j ⋅ h Pzz k k−1 j −1 Pxz k k−1 j T Pk k−1 j −1 rw k ð Þ 1⁄4 trace Qk−1 Pk k−1 j −1 Pxz k k−1 j P zz k k−1 j −1 ⋅ Pxz k k−1 j T Pk k−1 j −1 rz k ð Þ 1⁄4 trace E− Pxz k k−1 j T Pk k−1 j −1 Kk 8>>>><>>>>>>>>: ð39Þ Here, the covariance factors and the variance matrices for the nonlinear system can be calculated after the equations (15) ~ (18). Results and discussion Simulations and experiments In order to show the advantages of the improved CKF, this section presents the solution comparison between Fig. 3 PDF of state errors with improved CKF Zhang et al. The Journal of Global Positioning Systems (2017) 15:1 Page 6 of 9 the normal and the improved CKF methods through two nonlinear models: a bearing-only tracking model and a target tracking model, using the simulated data against their known reference solutions. A further experiment with the real data was also conducted and summarized. Bearing-only tracking method In the bearing-only tracking model, there are two states. Its nonlinear model was set as below (Zhang et al. 2015; Julier & Uhlman 1997; Kotecha & Djuric 2003): xk 1⁄4 0:9 0 0 1 xk−1 þ wk−1 ð40Þ where the state vector is x 1⁄4 x1 x2 1⁄2 T 1⁄4 s t 1⁄2 T , which are the position of a moving carrier in the 2D Cartesian coordinate system. The process noise is wk ~ N(0,Q) with